#include "ros2_cpp_node.h"

#include <fstream>

#include "ament_index_cpp/get_package_share_directory.hpp"
#include "grasp_item.h"
#include "is_item_available.h"
#include "log_info.h"
#include "navigate_to_shelf.h"
#include "navigate_to_sorting_area.h"
#include "return_to_charging_station.h"

SimpleNode::SimpleNode() : Node("warehouse_robot")
{
    // 1. 注册节点类型（保持不变）
    factory_.registerNodeType<GraspItem>("GraspItem", this);
    factory_.registerNodeType<IsItemAvailable>("IsItemAvailable", this);
    factory_.registerNodeType<LogInfo>("LogInfo", this);
    factory_.registerNodeType<NavigateToShelf>("NavigateToShelf", this);
    factory_.registerNodeType<NavigateToSortingArea>("NavigateToSortingArea", this);
    factory_.registerNodeType<ReturnToChargingStation>("ReturnToChargingStation", this);
    factory_.registerNodeType<BT::RetryNode>("Retry");

    // 2. 查找XML文件路径（保持不变）
    std::string package_path = ament_index_cpp::get_package_share_directory("btree_example");
    std::string xml_path = package_path + "/config/btree.xml";
    std::ifstream file(xml_path);
    if (!file.good())
    {
        RCLCPP_ERROR(get_logger(), "无法找到行为树XML文件: %s", xml_path.c_str());
        return;
    }

    // 3. 先创建行为树，再往【根黑板】写值（关键修改）
    tree_ = std::make_shared<BT::Tree>(factory_.createTreeFromFile(xml_path));

    auto root_blackboard = tree_->rootBlackboard();
    root_blackboard->set<int>("target_shelf", 5);  // 写入目标货架ID
    root_blackboard->set<int>("target_item", 42);  // 写入目标物品ID

    RCLCPP_INFO(get_logger(), "仓库机器人行为树初始化完成");
}